#include "../../headers/structsystem.h"

using namespace std;


int main()
{
    // create system
    StructSystem system = StructSystem(1);
    system.setJobname("example4");

    // solve parameters
    double endTime = 50.0;  // total time, s
    double zeta = 0.5;      // damping coefficient
    double h = 0.01;        // time step, s

    // create particles
    Particle* p1 = new Particle(1, 0.0, 10.0);
    Particle* p2 = new Particle(2, 0.0, 0.0);
    Particle* p3 = new Particle(3, 10.0, 0.0);
    Particle* p4 = new Particle(4, 2.5, 0.0);
    Particle* p5 = new Particle(5, 5.0, 0.0);
    Particle* p6 = new Particle(6, 7.5, 0.0);
    system.addParticle(p1);
    system.addParticle(p2);
    system.addParticle(p3);
    system.addParticle(p4);
    system.addParticle(p5);
    system.addParticle(p6);

    StdArray6d mass2 = {6.25, 6.25, 0, 0, 0, 0};
    p2->setLumpedMass(mass2);
    StdArray6d mass4 = {2.5, 2.5, 0, 0, 0, 0};
    p4->setLumpedMass(mass4);
    p5->setLumpedMass(mass4);
    p6->setLumpedMass(mass4);

    // create material
    LinearElastic mat1 = LinearElastic(1);
    mat1.setE(1.0E5);
    LinearElastic mat2 = LinearElastic(2);
    mat2.setE(1.0E4);

    // create section
    CustomSectionParas paras = {.A=1, .Sy=0, .Sz=0, .SHy=0, .SHz=0,
                                .CGy=0, .CGz=0.0, .Iyy=10, .Izz=10, .Iyz=10};
    CustomSection sect = CustomSection(1, paras);

    // create elements, flexible link
    Link2DLD* e1 = new Link2DLD(1, p1, p2, &mat1, &sect);
    Link2DLD* e2 = new Link2DLD(2, p2, p4, &mat2, &sect);
    Link2DLD* e3 = new Link2DLD(3, p4, p5, &mat2, &sect);
    Link2DLD* e4 = new Link2DLD(4, p5, p6, &mat2, &sect);
    Link2DLD* e5 = new Link2DLD(5, p6, p3, &mat2, &sect);
    system.addElement(e1);
    system.addElement(e2);
    system.addElement(e3);
    system.addElement(e4);
    system.addElement(e5);

    // constraints
    DOF c1 = {.key = {true, true, true, true, true, true},
              .val = {0, 0, 0, 0, 0, 0}};
    DOF c3 = {.key = {true, true, true, true, true, true},
              .val = {0, 0, 0, 0, 0, 0}};
    system.addConstraint(1, c1);
    system.addConstraint(3, c3);

    // save model
    string path = system.workdir() + "/" + system.jobname() + "/model";
    system.saveModel(path);
    system.info();      // print structsystem information

    // get the direction vector of bar1
    const Eigen::Vector3d* ex = e1->ex();
    StdArray6d P2 = {0, 0, 0, 0, 0, 0};
    StdArray6d Pv = {0, -0.1, 0, 0, 0, 0};

    int nStep = ceil(endTime/h);
    cout << nStep << endl;
    int interval = ceil(0.1/h);
    for (int i = 0; i <= nStep; i++)
    {
        // update P2
        P2[0] = 10 * (*ex)(0);
        P2[1] = -10 * (*ex)(1);
        if (i == 0)
        {
            // add external force
            system.addExternalForce(2, P2);
            system.addExternalForce(4, Pv);
            system.addExternalForce(5, Pv);
            system.addExternalForce(6, Pv);
            system.solve(h, zeta, true);
            system.setInternalForce();
        }
        else
        {
            system.solve(h, zeta);
            system.clearParticleForce();
            // add external force
            system.addExternalForce(2, P2);
            system.addExternalForce(4, Pv);
            system.addExternalForce(5, Pv);
            system.addExternalForce(6, Pv);
            system.setInternalForce();
        }

        // save results
        if (i % interval == 0)
        {
            string path = system.workdir() + "/" + system.jobname() + "/" +
                          to_string(i);
            system.saveModel(path);
            system.saveParticleResult(path);
            system.saveElementResult(path);
            system.saveSupportReact(path);
        }
    }

    system.releaseContainers();
    return 0;
}